#include "boost\math\quaternion.hpp"
//#include <boost\math\quaternion.hpp>
#include "DMath.h"
#include "../Tools/nvMath.h"

#define DEPSILON 0.00001f

using namespace nv;

template<typename T>
class qtx: public boost::math::quaternion<T>
{
public:
	void getRotationMatrix(/*matrix4<T> & mtx */ T *mtx)
	{
		// q3 implementation (quat_t const &src, mat3_t &dst)
		T	wx, wy, wz;
		T	xx, yy, yz;
		T	xy, xz, zz;
		T	x2, y2, z2;

		x2 = a + a;
		y2 = b + b;
		z2 = c + c;

		xx = a * x2;
		xy = a * y2;
		xz = a * z2;

		yy = b * y2;
		yz = b * z2;
		zz = c * z2;

		wx = d * x2;
		wy = d * y2;
		wz = d * z2;

		/*
		mtx(0,0) = T(1.0) - ( yy + zz );
		mtx(0,1) = xy - wz;
		mtx(0,2) = xz + wy;

		mtx(1,0) = xy + wz;
		mtx(1,1) = T(1.0) - ( xx + zz );
		mtx(1,2) = yz - wx;

		mtx(2,0) = xz - wy;
		mtx(2,1) = yz + wx;
		mtx(2,2) = T(1.0) - ( xx + yy );*/

		mtx[0] = T(1.0) - ( yy + zz );
		mtx[4] = xy - wz;
		mtx[8] = xz + wy;

		mtx[1] = xy + wz;
		mtx[5] = T(1.0) - ( xx + zz );
		mtx[9] = yz - wx;

		mtx[2] = xz - wy;
		mtx[6] = yz + wx;
		mtx[10] = T(1.0) - ( xx + yy );

		/*
		T s, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;

        T norm = a * a + b * b + c * c + d * d;
        s = ( norm == T(0.0)) ? T(0.0) : ( T(2.0) / norm );
		xs = a * s;
        ys = b * s;
        zs = c * s;

        wx = d * xs;
        wy = d * ys;
        wz = d * zs;

        xx = a * xs;
        xy = a * ys;
        xz = a * zs;

        yy = b * ys;
        yz = b * zs;
        zz = c * zs;

        mtx(0,0) = T( T(1.0) - ( yy + zz ));
        mtx(1,0) = T ( xy + wz );
        mtx(2,0) = T ( xz - wy );

        mtx(0,1) = T ( xy - wz );
        mtx(1,1) = T ( T(1.0) - ( xx + zz ));
        mtx(2,1) = T ( yz + wx );

        mtx(0,2) = T ( xz + wy );
        mtx(1,2) = T ( yz - wx );
        mtx(2,2) = T ( T(1.0) - ( xx + yy ));

        mtx(3,0) = mtx(3,1) = mtx(3,2) = mtx(0,3) = mtx(1,3) = mtx(2,3) = T(0.0);
        mtx(3,3) = T(1.0);*/
	}


	//======
	/*quaternion & */void fromAxisAngle( const vec3<T> &axis, T theta )
    {
		
        T sqnorm = square_norm(axis);

        if (sqnorm == T(0.0))
        {
            // axis too small.
            a = b = c = T(0.0);
            d = T(1.0);
        } 
        else 
        {
            theta *= T(0.5);
            T sin_theta = T(sin(theta));

            if ( sqnorm != T(1)) 
                sin_theta /= T(sqrt(sqnorm));
            a = sin_theta * axis[0];
            b = sin_theta * axis[1];
            c = sin_theta * axis[2];
            d = T(cos(theta));
        }
		
		/*T sin_theta;

		sin_theta = sin(theta);

		a = sin_theta * axis[0];
		b = sin_theta * axis[1];
		c = sin_theta * axis[2];
		d = T(cos(theta));*/
    }


	///

	qtx(T x, T y, T z, T w)
	{
		a = x;
		b = y;
		c = z;
		d = w;
	}

	qtx()
	{
		boost::math::quaternion<float>();
	}

	void setValues(T x, T y, T z, T w)
	{
		a = x;
		b = y;
		c = z;
		d = w;
	}


	void mult_vec( const vec3<T> &src, vec3<T> &dst ) const
    {
		/*vec3 qv(a, b, c);
		vec3 uv = cross(qv, src);
		vec3 uuv = cross(qv, uv) * 2.0f;
		uv *= 2.0f * q.w;**/


		
		//qtx v, qres;
		//v.setValues(src.x, src.y, src.z, 0.0f);

		//v /= *this;		// v /= *this    ===     v = v * inverse(*this)

		/*qres = *this * v;

		dst.x = qres.a;
		dst.y = qres.b;
		dst.z = qres.c;*/
    }

	friend qtx<T> operator * ( const qtx<T> & lhs, const qtx<T> & rhs ) {	
        qtx<T> r;
		
		/*r.setValues(lhs.a, lhs.b, lhs.c, lhs.d);
        r *= rhs; */

		/*r.a = lhs.d * rhs.a + lhs.a * rhs.d + lhs.b * rhs.c - lhs.c * rhs.b;
		r.b = lhs.d * rhs.b + lhs.b * rhs.d + lhs.c * rhs.a - lhs.a * rhs.c;
		r.c = lhs.d * rhs.c + lhs.c * rhs.d + lhs.a * rhs.b - lhs.b * rhs.a;
		r.d = lhs.d * rhs.d - lhs.a * rhs.a - lhs.b * rhs.b - lhs.c * rhs.d;*/
		//w * rq.x + x * rq.w + y * rq.z - z * rq.y,       w * rq.y + y * rq.w + z * rq.x - x * rq.z,       w * rq.z + z * rq.w + x * rq.y - y * rq.x,      w * rq.w - x * rq.x - y * rq.y - z * rq.z)
		T A, B, C, D, E, F, G, H;

		A = (lhs.d + lhs.a) * (rhs.d + rhs.a);
		B = (lhs.c - lhs.b) * (rhs.b - rhs.c);
		C = (lhs.a - lhs.d) * (rhs.b + rhs.c);
		D = (lhs.b + lhs.c) * (rhs.a - rhs.d);
		E = (lhs.a + lhs.c) * (rhs.a + rhs.b);
		F = (lhs.a - lhs.c) * (rhs.a - rhs.b);
		G = (lhs.d + lhs.b) * (rhs.d - rhs.c);
		H = (lhs.d - lhs.b) * (rhs.d + rhs.c);

		r.d = B + (-E - F + G + H) * 0.5f;
		r.a = A - ( E + F + G + H) * 0.5f; 
		r.b =-C + ( E - F + G - H) * 0.5f;
		r.c =-D + ( E - F - G + H) * 0.5f;

		return r;

    }

	inline float length()
	{
		float length;
	
		length = a * a + b * b + c * c + d * d;
		return ( float )sqrt( length );
	}

	inline void normalize()
	{
		float length;
		float ilength;

		length = this->Length();
		
		if ( length ) 
		{
			ilength = 1 / length;
			a *= ilength;
			b *= ilength;
			c *= ilength;
			d *= ilength;
		}
		/*
		float mag;

		mag = a*a + b*b + c*c + d*d;

		if(abs(mag) > DEPSILON  &&  abs(mag - 1.0f) > DEPSILON)
		{
			a /= mag;
			b /= mag;
			c /= mag;
			d /= mag;
		}*/
	}
};

class boostCam
{
public:
	qtx<float> rotX;
	qtx<float> rotY;
	qtx<float> rotZ;
	qtx<float> qrot;

	float pitch;
	float yaw;

	vec3f location;

	vec3f right;
	vec3f direction;
	vec3f up;

	boostCam()
	{
		rotX.setValues(0.0f, 0.0f, 0.0f, 1.0f);
		rotY.setValues(0.0f, 0.0f, 0.0f, 1.0f);
		rotZ.setValues(0.0f, 0.0f, 0.0f, 1.0f);
		qrot.setValues(0.0f, 0.0f, 0.0f, 1.0f);

		pitch = 0.0f;
		yaw = 0.0f;

		location.x = 0.0f;
		location.y = 0.0f;
		location.z = 0.0f;

		direction.x = 0.0f;
		direction.y = 0.0f;
		direction.z = 1.0f;

		right.x = 1.0f;
		right.y = 0.0f;
		right.z = 0.0f;

		up.x = 0.0f;
		up.y = 1.0f;
		up.z = 0.0f;
	}

	void GetMatrix(matrix4f & mtx)
	{
		qtx<float> res;

		res = rotX * rotY;
		res.getRotationMatrix(mtx._array);
	
		//qrot.getRotationMatrix(mtx);

		//qrot = res;

		// update direction
		direction.x = mtx._13;
		direction.y = mtx._23;
		direction.z = mtx._33;
		

		// update right
		right.x = mtx._11;
		right.y = mtx._21;
		right.z = mtx._31;

		// update up
		up.x = mtx._12;
		up.y = mtx._22;
		up.z = mtx._32;
	}


	void Pitch(float angle)
	{
		vec3f vc;
		vc.x = 1.0f;
		vc.y = 0.0f;
		vc.z = 0.0f;

		pitch += angle;
		rotX.fromAxisAngle(vc, pitch);

		qtx<float> q;
		q.fromAxisAngle(right, angle);
		//q.Normalize();
		qrot *= q;

		//qrot.Normalize();
	}

	void Yaw(float angle)
	{
		vec3f vc;
		vc.x = 0.0f;
		vc.y = -1.0f;
		vc.z = 0.0f;

		yaw += angle;
		rotY.fromAxisAngle(vc, yaw);

		qtx<float> q;
		q.fromAxisAngle(vc, angle);
		//q.Normalize();
		qrot *= q;
		//qrot.Normalize();
	}

	void Move(float move)
	{
		location.x += direction.x * move;
		location.y += direction.y * move;
		location.z += direction.z * move;	
	}

	void Strafe(float val)
	{
		location.x += -right.x * val;
		location.y += 0.0f;//right.y * val;
		location.z += -right.z * val;
	}

};